/*
 *   Copyright (C) 2011 David Swords
 *	
 *   This program is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation, either version 3 of the License, or
 *   (at your option) any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

package sensor;

import java.util.Vector;

import locate.Pose;
import comm.Communicator;
import comm.Parser;
import format.NumberConverter;

public class SensorReader {
	
	private final String class_details = "SensorReader.";
	private Communicator comm;
	private NumberConverter conv;
	private Parser parser;
	private Pose pose;
	
	public SensorReader(String IP) {
		final String constructor_details = class_details + "SensorReader(String)";
		chk(IP.isEmpty(), "The string IP is empty.", constructor_details);
		comm  = new Communicator(IP);
		conv = new NumberConverter();
		pose = new Pose(0.0, 0.0, 0.0);
		parser = new Parser();
		chk(comm == null, "Communicator is equal to null, please initilise.", constructor_details);
		chk(conv == null, "NumberCoverter is equal to null, please initilise.", constructor_details);
		chk(parser == null, "Parser is equal to null, please initilise.", constructor_details);
		chk(pose == null, "Pose is equal to null, please initilise.", constructor_details);
		comm.connect();
	}
	
	private void chk(final boolean expression, final String message, final String location) {
		if(expression) {
			System.err.println("#############################################################");
			System.err.println(message);
			System.err.println(location);
			System.err.println("#############################################################");
			System.exit(1);
		}
	}
	
	public boolean isConnected() {
		final String method_details = class_details + "isConnected() returns boolean";
		chk(comm == null, "Communicator is equal to null, please initialize.", method_details);
		return comm.isConnected();
	}
	
	public Pose pose() {
		final String method_details = class_details + "pose() returns void";
		chk(comm == null, "Communicator is equal to null, please initialise.", method_details);
		chk(parser == null, "Parser is equal to null, please initialise.", method_details);
		chk(pose == null, "Pose is equal to null, please initialise.", method_details);
		comm.write("r xx x040D x040E x040F x000 x000 \n");
		Vector< String > temp = parser.build(comm.read());
		chk(temp == null, "The Vector< String > temp is equal to null, please initialise.", method_details);
		chk(temp.isEmpty(), "The Vector< String > temp is empty.", method_details);
		
		if(temp.size() == 10 && temp.get(0).equals("<<") && temp.get(1).equals("5") && temp.get(2).equals("R")) {
			double dec_x = conv.systemToDirection(conv.hexToDec(temp.get(4)));
			double dec_y = conv.systemToDirection(conv.hexToDec(temp.get(5)));
			double dec_t = conv.systemToDirectionRadians(conv.hexToDec(temp.get(6)));
			pose =  new Pose(dec_x, dec_y, dec_t);
		} 
		
		chk(pose == null, "Pose is equal to null, please initialise.", method_details);
		
		return pose;
	}
	
	public void disconnect() {
		final String method_details = class_details + "disconnect() returns void";
		chk(comm == null, "Communicator is equal to null, please initialise.", method_details);
		comm.disconnect();
	}
}
